/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-10-08     huger       the first version
 */
#include "app_move.h"
#include "app_io.h"
#include "app_imu.h"

#include "motor/motor.h"
#include "math/micro_adjust_cal.h"

#include "bsp/bsp_can1.h"
#include "bsp/bsp_adc.h"


#define DBG_TAG "move"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

static rt_timer_t timer;
static rt_sem_t sem;


// 定义三轴微调平台坐标系下的标定IMU的坐标系的旋转矩阵 R
array<array<double, 3>, 3> rotationMatrix_wTb = { {
    {0, 0, -1},
    {1, 0, 0},
    {0, -1, 0}
} };

// 定义标定IMU的坐标系下的三轴微调平台坐标系的旋转矩阵（转置）
array<array<double, 3>, 3> rotationMatrix_bTw = { {
    {0, 1, 0},
    {0, 0, -1},
    {-1, 0, 0}
} };

//定义固定IMU坐标系下的镜杆坐标系的旋转矩阵(对内窥镜的摆放应做规定，规定内窥镜正方向）
array<array<double, 3>, 3> rotationMatrix_gTj = { {
    {1, 0, 0},
    {0, 0, -1},
    {0, 1, 0}
} };

//定义镜杆坐标系下的固定IMU坐标系的旋转矩阵(对内窥镜的摆放应做规定，规定内窥镜正方向）
array<array<double, 3>, 3> rotationMatrix_jTg = { {
    {1, 0, 0},
    {0, 0, 1},
    {0, -1, 0}
} };

//定义镜杆坐标系到相机坐标系的旋转矩阵（对于30度镜子）
//相当于相机坐标系绕镜杆坐标系的x轴旋转30度。
array<array<double, 3>, 3> rotationMatrix_jTc = { {
    {1, 0, 0},
    {0, 0.866, -0.5},
    {0, 0.5, 0.866}
} };

array<array<double, 3>, 3> rotationMatrix_cTj = { {
    {1, 0, 0},
    {0, 0.866, 0.5},
    {0, -0.5, 0.866}
} };







static void timeout (void *parameter)
{
    rt_sem_release(sem);
}

// 线程入口
static void thread_entry(void* parameter)
{
    float rad[3]={0}; //roll  pitch  yaw   翻滚   俯仰  偏航
    iomap map = {0};

    MathUtils micro_math;
    MicroAdjustment microAdjust;

    array<double,3>Move_Distance;
    array<double,3>disp_preview;

    motor_init();  //电机参数初始化
    if(micro_adjust_platform_selfcheck()==0)return; //微调平台自检

    bsp_adc_init();

    bsp_can1_init();  //can初始化

    // 创建信号量
    sem = rt_sem_create("angle sem", 0, RT_IPC_FLAG_FIFO);
    // 创建定时器  周期定时器
    timer = rt_timer_create("angle time", timeout,
                             RT_NULL,
                             rt_tick_from_millisecond(200),
                             RT_TIMER_FLAG_PERIODIC);

    if (timer != RT_NULL)
        rt_timer_start(timer);

    LOG_D("micro_adjust_pallet init success");

    while (1)
    {
        if(rt_sem_take(sem, RT_WAITING_FOREVER) != RT_EOK)
            continue;

        microAdjust.adjust_dir = NONE;

        app_io_get_gpio_map(&map); //获取按键状态
        imu_angel_get(rad);//获取imu角度

        if(map.key_st[2])   //进入标定程序
        {
            microAdjust.cacli_status = 1;

            for(int cont =0;cont<3;cont++)
            {
                microAdjust.disp[cont]=0;
                disp_preview[cont]=0;
            }
            micro_math.rotationMatrix_euler0 = micro_math.eulerToRotationMatrix(rad[0], rad[1], rad[2]);
            micro_math.rotationMatrix_euler0_inv = micro_math.rotationMatrixInverse(micro_math.rotationMatrix_euler0);

            LOG_D("坐标系标定 : %.1f,%.1f,%.1f \r\n",rad[0],rad[1],rad[2]);

        }
        else if((map.key_st[3]) && (microAdjust.cacli_status))  //防误碰按键
        {
            for(int i=0;i<3;i++)microAdjust.disp[i]=0;

            if(map.key_st[1])   //z+
            {
                microAdjust.disp[2] = +0.05;
                microAdjust.adjust_dir = IN;
                LOG_D("move in");
            }//
            else if (map.key_st[0]) //z-
            {
                microAdjust.disp[2] = -0.05;
                microAdjust.adjust_dir = OUT;
                LOG_D("move out");
            }

            if(adc_buff[0]<=(2048-Threshold_Value))//x
            {
                microAdjust.disp[1] = 0.05;
                microAdjust.adjust_dir = UP;
                LOG_D("move up");
            }
            else if(adc_buff[0]>=(2048+Threshold_Value))
            {
                microAdjust.disp[1] = -0.05;
                microAdjust.adjust_dir = DOWN;
                LOG_D("move down");
            }

            if(adc_buff[1]<=(2048-Threshold_Value))   //y
            {
                microAdjust.disp[0] = 0.05;
                microAdjust.adjust_dir = LEFT;
                LOG_D("move left");
            }
            else if(adc_buff[1]>=(2048+Threshold_Value))
            {
                microAdjust.disp[0] = -0.05;
                microAdjust.adjust_dir = RIGHT;
                LOG_D("move right");
            }

            if((microAdjust.adjust_dir != NONE) && (microAdjust.cacli_status))
            {
                //欧拉角转旋转矩阵
                micro_math.rotationMatrix_euler1 = micro_math.eulerToRotationMatrix(rad[0], rad[1], rad[2]);

                //计算相对旋转矩阵（固定处姿态在标定处姿态下的姿态表示）
                array<array<double, 3>, 3> rotationMatrix_bTg = micro_math.matrixMultiply(micro_math.rotationMatrix_euler0_inv,micro_math.rotationMatrix_euler1);

                //计算固定IMU的坐标系到三轴微调平台坐标系的旋转矩阵
                array<array<double, 3>, 3> rotationMatrix_wTg = micro_math.matrixMultiply(rotationMatrix_bTw, rotationMatrix_bTg);

                //计算固定内窥镜坐标系到三轴微调平台坐标系的旋转矩阵（0°镜子适用）
                microAdjust.R = micro_math.matrixMultiply(rotationMatrix_wTg, rotationMatrix_jTg);

                //计算固定内窥镜坐标系到三轴微调平台坐标系的旋转矩阵（30°镜子使用）
                microAdjust.R = micro_math.matrixMultiply(microAdjust.R, rotationMatrix_jTc);

                for(int i=0;i<3;i++)
                {
                    LOG_D("%.5f,%.5f,%.5f",microAdjust.R[i][0],microAdjust.R[i][1],microAdjust.R[i][2]);
                }
                LOG_D("%.2f,%.2f,%.2f",microAdjust.disp[0],microAdjust.disp[1],microAdjust.disp[2]);
                // 计算微调装置坐标系下的移动量，微调装置的位移量
                Move_Distance = microAdjust.calculateMovement(microAdjust.disp); //计算相对于零点,各个轴移动的距离

                LOG_D("rad:%.5f,%.5f,%.5f dis:%.5f,%.5f,%.5f\r\n",rad[0], rad[1], rad[2],Move_Distance[0],Move_Distance[1],Move_Distance[2]);

                for(int var =0; var<3; var++)
                {
                    if(InterPolation_Move(var, interpolation_para[var].endpoint_pulse + (double_t)Move_Distance[var]/ screw_lead[var] * 1600, 220 ))
//                    if(InterPolation_Move(var,microAdjust.disp[var] * RAD  /360 * motor_subdivision,210))
                    {
                        LOG_D("motor: %d, tar: %d, cur: %d",var,interpolation_para[var].endpoint_pulse,interpolation_para[var].currentpoint_pluse);
                    }
                    else
                    {
                        for(int i =0;i<3;i++)srd[i].run_state = STOP;
                        break;
                    }
                }
            }
        }
    }
}


void app_move_init(void)
{
    #define THREAD_STACK_SIZE   4096
    #define THREAD_PRIORITY     9
    #define THREAD_TIMESLICE    9

    rt_thread_t tid = RT_NULL;

    // 创建线程
    tid = rt_thread_create("move control",
                            thread_entry, RT_NULL,
                            THREAD_STACK_SIZE,
                            THREAD_PRIORITY, THREAD_TIMESLICE);


    if (tid != RT_NULL)
        rt_thread_startup(tid);
}


